#include "qlib/geo/curve.h"

namespace qlib {
LineElement::LineElement() {
  memset((void*)this, 0, sizeof(LineElement));
  half_width_and_offset = Vec2dArray();
  end_extensions = Vec2d();
}

PXXData::PXXData() {
  memset((void*)this, 0, sizeof(PXXData));
}

RaithData::RaithData() {
  memset((void*)this, 0, sizeof(RaithData));
  base_cell_name = std::string();
}

Curve::Curve() {
  tolerance = 0;
  owner = nullptr;
}

CurveInstruction::CurveInstruction() {
  memset((void*)this, 0, sizeof(CurveInstruction));
}

void PXXData::little_endian_swap() {
  little_endian_swap16((uint16_t*)(this + offsetof(PXXData, unused)), 1);
  little_endian_swap64((uint64_t*)(this + offsetof(PXXData, pitch_parallel_to_path)), 3);
  little_endian_swap32((uint32_t*)(this + offsetof(PXXData, periods)), 5);
  little_endian_swap64((uint64_t*)(this + offsetof(PXXData, ret_stage_speed)), 2);
  little_endian_swap16((uint16_t*)(this + offsetof(PXXData, revision)), 1);
}

PXXData RaithData::to_pxxdata() const {
  PXXData pd{};
  pd.dwelltime_selection = dwelltime_selection;
  pd.pitch_parallel_to_path = pitch_parallel_to_path;
  pd.pitch_perpendicular_to_path = pitch_perpendicular_to_path;
  pd.pitch_scale = pitch_scale;
  pd.periods = periods;
  pd.grating_type = grating_type;
  pd.dots_per_cycle = dots_per_cycle;
  memset(pd.free, 0, sizeof(pd.free));
  pd.revision = 1;
  return pd;
}

void RaithData::from_pxxdata(PXXData const& pxxdata) {
  pitch_parallel_to_path = pxxdata.pitch_parallel_to_path;
  pitch_perpendicular_to_path = pxxdata.pitch_perpendicular_to_path;
  pitch_scale = pxxdata.pitch_scale;
  periods = pxxdata.periods;
  grating_type = pxxdata.grating_type;
  dots_per_cycle = pxxdata.dots_per_cycle;
  dwelltime_selection = pxxdata.dwelltime_selection;
}

void Curve::init(const Vec2d initial_position, double tolerance_) {
  points.push_back(initial_position);
  tolerance = tolerance_;
}

void Curve::append(const Vec2d v) {
  points.push_back(v);
}

void Curve::append_unsafe(const Vec2d v) {
  points.push_back(v);
}

void Curve::remove(uint64_t index) {
  array_remove(points, index);
}

int Curve::ensure_slots(uint64_t free_slots) {
  return array_ensure_slots(points, free_slots);
}

bool Curve::closed() const {
  if (points.size() < 2)
    return false;
  const Vec2d v = points[0] - points.back();
  return v.length_sq() <= tolerance * tolerance;
}

void Curve::horizontal(double coord_x, bool relative) {
  last_ctrl = points.back();
  if (relative) {
    Vec2d v = last_ctrl;
    v.x += coord_x;
    points.push_back(v);
  } else {
    points.push_back(Vec2d{coord_x, last_ctrl.y});
  }
}
void Curve::horizontal(const Array<double> coord_x, bool relative) {
  int old_size = vector_resize_now(points, coord_x.size());
  Vec2d* dst = points.data() + old_size;
  const double* src_x = coord_x.data();
  if (relative) {
    const Vec2d ref = points[old_size - 1];
    for (uint64_t i = 0; i < coord_x.size(); i++, dst++) {
      dst->x = ref.x + *src_x++;
      dst->y = ref.y;
    }
  } else {
    const double ref_y = points[old_size - 1].y;
    for (uint64_t i = 0; i < coord_x.size(); i++, dst++) {
      dst->x = *src_x++;
      dst->y = ref_y;
    }
  }
  last_ctrl = points[points.size() - 2];
}
void Curve::vertical(double coord_y, bool relative) {
  last_ctrl = points.back();
  if (relative) {
    Vec2d v = last_ctrl;
    v.y += coord_y;
    points.push_back(v);
  } else {
    points.push_back(Vec2d{last_ctrl.x, coord_y});
  }
}
void Curve::vertical(const Array<double> coord_y, bool relative) {
  int old_size = vector_resize_now(points, coord_y.size());
  Vec2d* dst = points.data() + old_size;
  const double* src_y = coord_y.data();
  if (relative) {
    const Vec2d ref = points[old_size - 1];
    for (uint64_t i = 0; i < coord_y.size(); i++, dst++) {
      dst->x = ref.x;
      dst->y = ref.y + *src_y++;
    }
  } else {
    const double ref_x = points[old_size - 1].x;
    for (uint64_t i = 0; i < coord_y.size(); i++, dst++) {
      dst->x = ref_x;
      dst->y = *src_y++;
    }
  }
  last_ctrl = points[points.size() - 2];
}
void Curve::segment(Vec2d end_point, bool relative) {
  last_ctrl = points[points.size() - 1];
  if (relative) {
    points.push_back(end_point + last_ctrl);
  } else {
    points.push_back(end_point);
  }
}
void Curve::segment(const Array<Vec2d> _points, bool relative) {
  if (relative) {
    int old_size = vector_resize_now(points, _points.size());

    const Vec2d ref = points[old_size - 1];
    const Vec2d* src = _points.data();
    Vec2d* dst = points.data() + old_size;
    for (uint64_t i = 0; i < _points.size(); i++)
      *dst++ = ref + *src++;
  } else {
    array_extend(points, _points);
  }
  last_ctrl = points[points.size() - 2];
}

void Curve::cubic(const Array<Vec2d> _points, bool relative) {
  Vec2d last_point = points.back();
  if (relative) {
    const Vec2d ref = points.back();
    for (uint64_t i = 0; i < _points.size() - 2; i += 3) {
      Vec2d first_point = last_point;
      last_point = ref + _points[i + 2];
      append_cubic(first_point, ref + _points[i], ref + _points[i + 1], last_point);
    }
    last_ctrl = ref + _points[_points.size() - 2];
  } else {
    for (uint64_t i = 0; i < _points.size() - 2; i += 3) {
      Vec2d first_point = last_point;
      last_point = _points[i + 2];
      append_cubic(first_point, _points[i], _points[i + 1], last_point);
    }
    last_ctrl = _points[_points.size() - 2];
  }
}

void Curve::cubic_smooth(const Array<Vec2d> _points, bool relative) {
  Vec2d last_point = points.back();
  const Vec2d* point = _points.data();
  if (relative) {
    const Vec2d ref = points.back();
    for (uint64_t i = 0; i < _points.size() - 1; i += 2) {
      Vec2d first_point = last_point;
      Vec2d smooth_ctrl = last_point * 2 - last_ctrl;
      last_ctrl = ref + *point++;
      last_point = ref + *point++;
      append_cubic(first_point, smooth_ctrl, last_ctrl, last_point);
    }
  } else {
    for (uint64_t i = 0; i < _points.size() - 1; i += 2) {
      Vec2d first_point = last_point;
      Vec2d smooth_ctrl = last_point * 2 - last_ctrl;
      last_ctrl = *point++;
      last_point = *point++;
      append_cubic(first_point, smooth_ctrl, last_ctrl, last_point);
    }
  }
}

void Curve::quadratic(const Array<Vec2d> _points, bool relative) {
  Vec2d last_point = points.back();
  if (relative) {
    const Vec2d ref = points.back();
    for (uint64_t i = 0; i < _points.size() - 1; i += 2) {
      Vec2d first_point = last_point;
      last_point = ref + _points[i + 1];
      append_quad(first_point, ref + _points[i], last_point);
    }
    last_ctrl = ref + _points[_points.size() - 2];
  } else {
    for (uint64_t i = 0; i < _points.size() - 1; i += 2) {
      Vec2d first_point = last_point;
      last_point = _points[i + 1];
      append_quad(first_point, _points[i], last_point);
    }
    last_ctrl = _points[_points.size() - 2];
  }
}

void Curve::quadratic_smooth(Vec2d end_point, bool relative) {
  const Vec2d first_point = points.back();
  last_ctrl = first_point * 2 - last_ctrl;
  if (relative) {
    append_quad(first_point, last_ctrl, first_point + end_point);
  } else {
    append_quad(first_point, last_ctrl, end_point);
  }
}

void Curve::quadratic_smooth(const Array<Vec2d> _points, bool relative) {
  Vec2d last_point = points.back();
  const Vec2d* point = _points.data();
  if (relative) {
    const Vec2d ref = points.back();
    for (uint64_t i = 0; i < _points.size(); i += 1) {
      Vec2d first_point = last_point;
      last_ctrl = last_point * 2 - last_ctrl;
      last_point = ref + *point++;
      append_quad(first_point, last_ctrl, last_point);
    }
  } else {
    for (uint64_t i = 0; i < _points.size(); i += 1) {
      Vec2d first_point = last_point;
      last_ctrl = last_point * 2 - last_ctrl;
      last_point = *point++;
      append_quad(first_point, last_ctrl, last_point);
    }
  }
}

void Curve::bezier(const Array<Vec2d> _points, bool relative) {
  Array<Vec2d> ctrl = {};
  int old_size = vector_resize_now(ctrl, _points.size() + 1);
  ctrl.back() = old_size;

  if (relative) {
    const Vec2d* point = _points.data();
    const Vec2d ref = points[points.size() - 1];
    ctrl[0] = ref;
    Vec2d* dst = ctrl.data() + 1;
    for (uint64_t i = 0; i < _points.size(); i++, dst++)
      *dst = ref + *point++;
  } else {
    ctrl[0] = points.back();
    memcpy(ctrl.data() + 1, _points.data(), sizeof(Vec2d) * _points.size());
  }
  append_bezier(ctrl);
  last_ctrl = _points[_points.size() - 2];
  ctrl.clear();
}

void Curve::interpolation(const Array<Vec2d> _points, double* angles, bool* angle_constraints, Vec2d* tension, double initial_curl, double final_curl,
                          bool cycle, bool relative) {
  Gds_Array<Vec2d> hobby_vec;
  hobby_vec.items = new Vec2d[(3 * (points.size() + 1) + 1)];

  const Vec2d ref = points.back();
  const Vec2d* src = _points.data();
  Vec2d* dst = hobby_vec.items + 3;
  hobby_vec.items[0] = ref;
  if (relative) {
    for (uint64_t i = 0; i < _points.size(); i++, dst += 3)
      *dst = ref + *src++;
  } else {
    for (uint64_t i = 0; i < _points.size(); i++, dst += 3)
      *dst = *src++;
  }
  hobby_interpolation(_points.size() + 1, hobby_vec.items, angles, angle_constraints, tension, initial_curl, final_curl, cycle);

  Gds_Array<Vec2d> tmp;
  tmp.items = hobby_vec.items + 1;

  if (cycle) {
    tmp.count = 3 * (points.size() + 1);
    hobby_vec.items[3 * (points.size() + 1)] = ref;
  } else {
    tmp.count = 3 * points.size();
  }

  Vec2dArray tmp_t;
  tmp_t.resize(tmp.count);
  for (size_t i = 0; i < tmp.count; i++) {
    tmp_t[i] = tmp.items[i];
  }

  cubic(tmp_t, false);
  delete[] hobby_vec.items;
}

void Curve::arc(double radius_x, double radius_y, double initial_angle, double final_angle, double rotation) {
  const double full_angle = fabs(final_angle - initial_angle);
  const double max_radius = radius_x > radius_y ? radius_x : radius_y;
  int64_t num_points = 1 + arc_num_points(full_angle, max_radius, tolerance);
  if (num_points < GDSTK_MIN_POINTS)
    num_points = GDSTK_MIN_POINTS;

  initial_angle = elliptical_angle_transform(initial_angle - rotation, radius_x, radius_y);
  final_angle = elliptical_angle_transform(final_angle - rotation, radius_x, radius_y);
  const double cr = cos(rotation);
  const double sr = sin(rotation);
  double x = radius_x * cos(initial_angle);
  double y = radius_y * sin(initial_angle);
  const Vec2d point0 = {x * cr - y * sr, x * sr + y * cr};
  const Vec2d delta = points.back() - point0;

  int old_size;

  if (num_points < 1)
    old_size = 0;
  else
    old_size = vector_resize_now(points, num_points - 1);

  Vec2d* dst = points.data() + old_size;
  for (uint64_t i = 1; i < num_points; i++) {
    double t = i / (num_points - 1.0);
    double angle = LERP(initial_angle, final_angle, t);
    x = radius_x * cos(angle);
    y = radius_y * sin(angle);
    Vec2d point = {x * cr - y * sr, x * sr + y * cr};
    *dst++ = point + delta;
  }

  Vec2d v = points[points.size() - 2] - points[points.size() - 1];
  v *= 0.5 * (radius_x + radius_y) / v.length();
  last_ctrl = points[points.size() - 1] + v;
}

void Curve::parametric(ParametricVec2 curve_function, void* data, bool relative) {
  const Vec2d last_curve_point = points.back();
  const Vec2d ref = relative ? last_curve_point : Vec2d{0, 0};
  const double tolerance_sq = tolerance * tolerance;
  double u = 0;
  Vec2d last = (*curve_function)(0, data) + ref;
  if ((last - last_curve_point).length_sq() > tolerance_sq)
    append(last);
  double du = 1.0 / GDSTK_MIN_POINTS;
  while (u < 1) {
    if (du > 1.0 / GDSTK_MIN_POINTS)
      du = 1.0 / GDSTK_MIN_POINTS;
    if (u + du > 1.0)
      du = 1.0 - u;
    Vec2d next = (*curve_function)(u + du, data) + ref;
    Vec2d mid = (*curve_function)(u + 0.5 * du, data) + ref;
    double err_sq = distance_to_line_sq(mid, last, next);
    if (err_sq <= tolerance_sq) {
      const Vec2d extra = (*curve_function)(u + du / 3, data) + ref;
      err_sq = distance_to_line_sq(extra, last, next);
    }
    while (err_sq > tolerance_sq) {
      du *= 0.5;
      next = mid;
      mid = (*curve_function)(u + 0.5 * du, data) + ref;
      err_sq = distance_to_line_sq(mid, last, next);
      if (err_sq <= tolerance_sq) {
        const Vec2d extra = (*curve_function)(u + du / 3, data) + ref;
        err_sq = distance_to_line_sq(extra, last, next);
      }
    }
    append(next);
    last = next;
    u += du;
    du *= 2;
  }
}

int64_t Curve::commands(const CurveInstruction* items, uint64_t count) {
  const CurveInstruction* item = items;
  const CurveInstruction* end = items + count;
  Array<Vec2d> points = {};
  while (item < end) {
    const char instruction = (item++)->command;
    switch (instruction) {
      case 'h':
      case 'H':
        if (end - item < 1)
          return item - items - 1;
        horizontal(*(double*)item, instruction == 'h');
        item += 1;
        break;
      case 'v':
      case 'V':
        if (end - item < 1)
          return item - items - 1;
        vertical(*(double*)item, instruction == 'v');
        item += 1;
        break;
      case 'l':
      case 'L':
        if (end - item < 2)
          return item - items - 1;
        segment(*(Vec2d*)item, instruction == 'l');
        item += 2;
        break;
      case 'c':
      case 'C':
        if (end - item < 6)
          return item - items - 1;
        points.resize(3);
        for (size_t i = 0; i < points.size(); i++) {
          points[i] = ((Vec2d*)item)[i];
        }

        cubic(points, instruction == 'c');
        item += 6;
        break;
      case 's':
      case 'S':
        if (end - item < 4)
          return item - items - 1;
        points.resize(2);
        for (size_t i = 0; i < points.size(); i++) {
          points[i] = ((Vec2d*)item)[i];
        }

        cubic_smooth(points, instruction == 's');
        item += 4;
        break;
      case 'q':
      case 'Q':
        if (end - item < 4)
          return item - items - 1;
        points.resize(2);
        for (size_t i = 0; i < points.size(); i++) {
          points[i] = ((Vec2d*)item)[i];
        }

        quadratic(points, instruction == 'q');
        item += 4;
        break;
      case 't':
      case 'T':
        if (end - item < 2)
          return item - items - 1;
        points.resize(1);
        for (size_t i = 0; i < points.size(); i++) {
          points[i] = ((Vec2d*)item)[i];
        }

        quadratic_smooth(points, instruction == 't');
        item += 2;
        break;
      case 'a':
        // Turn
        if (end - item < 2)
          return item - items - 1;
        turn(item->number, (item + 1)->number);
        item += 2;
        break;
      case 'A':
        // Circular arc
        if (end - item < 3)
          return item - items - 1;
        arc(item->number, item->number, (item + 1)->number, (item + 2)->number, 0);
        item += 3;
        break;
      case 'E':
        // Elliptical arc
        if (end - item < 5)
          return item - items - 1;
        arc(item->number, (item + 1)->number, (item + 2)->number, (item + 3)->number, (item + 4)->number);
        item += 5;
        break;
      default:
        return item - items - 1;
    }
  }
  return count;
}

void Curve::turn(double radius, double angle) {
  const Vec2d direction = points.back() - last_ctrl;
  double initial_angle = direction.angle() + (angle < 0 ? 0.5 * M_PI : -0.5 * M_PI);
  arc(radius, radius, initial_angle, initial_angle + angle, 0);
}

void Curve::append_cubic(const Vec2d p0, const Vec2d p1, const Vec2d p2, const Vec2d p3) {
  // Sampling based on curvature
  // dp* : 1st derivative
  const Vec2d dp0 = 3 * (p1 - p0);
  const Vec2d dp1 = 3 * (p2 - p1);
  const Vec2d dp2 = 3 * (p3 - p2);
  // d2p* : 2nd derivative
  const Vec2d d2p0 = 2 * (dp1 - dp0);
  const Vec2d d2p1 = 2 * (dp2 - dp1);

  const double tolerance_sq = tolerance * tolerance;
  Vec2d last = p0;
  double t = 0;
  while (t < 1) {
    Vec2d dc = eval_bezier2(t, dp0, dp1, dp2);
    Vec2d d2c = eval_line(t, d2p0, d2p1);
    double len_dc = dc.length();
    double dt = 0.5 / GDSTK_MIN_POINTS;
    if (len_dc > 0) {
      double curvature = fabs(dc.cross(d2c)) / (len_dc * len_dc * len_dc);
      if (curvature < GDSTK_PARALLEL_EPS) {
        dt = 1.0;
      } else {
        double angle = 2 * acos(1 - curvature * tolerance);
        dt = angle / (curvature * len_dc);
      }
    }
    if (t + dt > 1)
      dt = 1 - t;
    if (dt > 1.0 / GDSTK_MIN_POINTS)
      dt = 1.0 / GDSTK_MIN_POINTS;

    Vec2d next = eval_bezier3(t + dt, p0, p1, p2, p3);
    Vec2d mid = eval_bezier3(t + 0.5 * dt, p0, p1, p2, p3);
    double err_sq = distance_to_line_sq(mid, last, next);
    if (err_sq <= tolerance_sq) {
      const Vec2d extra = eval_bezier3(t + dt / 3, p0, p1, p2, p3);
      err_sq = distance_to_line_sq(extra, last, next);
    }
    while (err_sq > tolerance_sq) {
      dt *= 0.5;
      next = mid;
      mid = eval_bezier3(t + 0.5 * dt, p0, p1, p2, p3);
      err_sq = distance_to_line_sq(mid, last, next);
      if (err_sq <= tolerance_sq) {
        const Vec2d extra = eval_bezier3(t + dt / 3, p0, p1, p2, p3);
        err_sq = distance_to_line_sq(extra, last, next);
      }
    }

    append(next);
    last = next;
    t += dt;
  }
}

void Curve::append_quad(const Vec2d p0, const Vec2d p1, const Vec2d p2) {
  // Sampling based on curvature
  // dp* : 1st derivative
  const Vec2d dp0 = 2 * (p1 - p0);
  const Vec2d dp1 = 2 * (p2 - p1);
  // d2p* : 2nd derivative
  const Vec2d d2c = dp1 - dp0;

  const double tolerance_sq = tolerance * tolerance;
  Vec2d last = p0;
  double t = 0;
  while (t < 1) {
    Vec2d dc = eval_line(t, dp0, dp1);
    double len_dc = dc.length();
    double dt = 0.5 / GDSTK_MIN_POINTS;
    if (len_dc > 0) {
      double curvature = fabs(dc.cross(d2c)) / (len_dc * len_dc * len_dc);
      if (curvature < GDSTK_PARALLEL_EPS) {
        dt = 1.0;
      } else {
        double angle = 2 * acos(1 - curvature * tolerance);
        dt = angle / (curvature * len_dc);
      }
    }
    if (t + dt > 1)
      dt = 1 - t;
    if (dt > 1.0 / GDSTK_MIN_POINTS)
      dt = 1.0 / GDSTK_MIN_POINTS;

    Vec2d next = eval_bezier2(t + dt, p0, p1, p2);
    Vec2d mid = eval_bezier2(t + 0.5 * dt, p0, p1, p2);
    double err_sq = distance_to_line_sq(mid, last, next);
    if (err_sq <= tolerance_sq) {
      const Vec2d extra = eval_bezier2(t + 0.5 * dt, p0, p1, p2);
      err_sq = distance_to_line_sq(extra, last, next);
    }
    while (err_sq > tolerance_sq) {
      dt *= 0.5;
      next = mid;
      mid = eval_bezier2(t + 0.5 * dt, p0, p1, p2);
      err_sq = distance_to_line_sq(mid, last, next);
      if (err_sq <= tolerance_sq) {
        const Vec2d extra = eval_bezier2(t + 0.5 * dt, p0, p1, p2);
        err_sq = distance_to_line_sq(extra, last, next);
      }
    }

    append(next);
    last = next;
    t += dt;
  }
}
void Curve::append_bezier(const Array<Vec2d> ctrl) {
  const uint64_t count = ctrl.size();
  // Sampling based on curvature
  // dp : 1st derivative
  Gds_Array<Vec2d> dp;
  // d2p : 2nd derivative
  Gds_Array<Vec2d> d2p;

  int size = (count - 1 + count - 2) >= 1 ? (count - 1 + count - 2) : 1;
  dp.items = new Vec2d[count - 1 + count - 2];
  d2p.items = dp.items + count - 1;

  Vec2d* dst0 = dp.items;
  Vec2d* dst1 = d2p.items;
  const Vec2d* src = ctrl.data();
  for (uint64_t i = 0; i < count - 1; i++, src++, dst0++) {
    *dst0 = (double)(count - 1) * (*(src + 1) - *src);
    if (i > 0)
      *dst1++ = (double)(count - 2) * (*dst0 - *(dst0 - 1));
  }
  dp.count = count - 1;
  d2p.count = count - 2;

  const double tolerance_sq = tolerance * tolerance;
  const double dt_max = 1.0 / count;
  Vec2d last = ctrl[0];
  double t = 0;
  while (t < 1) {
    Vec2d dc = eval_bezier(t, dp.items, dp.count);
    Vec2d d2c = eval_bezier(t, d2p.items, d2p.count);
    double len_dc = dc.length();
    double dt = 0.5 * dt_max;
    if (len_dc > 0) {
      double curvature = fabs(dc.cross(d2c)) / (len_dc * len_dc * len_dc);
      if (curvature < GDSTK_PARALLEL_EPS) {
        dt = 1.0;
      } else {
        double angle = 2 * acos(1 - curvature * tolerance);
        dt = angle / (curvature * len_dc);
      }
    }
    if (t + dt > 1)
      dt = 1 - t;
    if (dt > dt_max)
      dt = dt_max;

    Vec2d next = eval_bezier(t + dt, ctrl.data(), ctrl.size());
    Vec2d mid = eval_bezier(t + 0.5 * dt, ctrl.data(), ctrl.size());
    double err_sq = distance_to_line_sq(mid, last, next);
    if (err_sq <= tolerance_sq) {
      const Vec2d extra = eval_bezier(t + 0.5 * dt, ctrl.data(), ctrl.size());
      err_sq = distance_to_line_sq(extra, last, next);
    }
    while (err_sq > tolerance_sq) {
      dt *= 0.5;
      next = mid;
      mid = eval_bezier(t + 0.5 * dt, ctrl.data(), ctrl.size());
      err_sq = distance_to_line_sq(mid, last, next);
      if (err_sq <= tolerance_sq) {
        const Vec2d extra = eval_bezier(t + 0.5 * dt, ctrl.data(), ctrl.size());
        err_sq = distance_to_line_sq(extra, last, next);
      }
    }

    append(next);
    last = next;
    t += dt;
  }
  delete[] dp.items;
}
}  // namespace qlib